{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy\n",
    "from sympy import (Symbol, MatrixSymbol, Matrix, symbols, simplify, eye, sin, cos)\n",
    "\n",
    "sympy.init_printing(use_latex=\"mathjax\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & - w_{2} & w_{1}\\\\w_{2} & 0 & - w_{0}\\\\- w_{1} & w_{0} & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[   0, -w_2,  w_1],\n",
       "[ w_2,    0, -w_0],\n",
       "[-w_1,  w_0,    0]])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "w = symbols('w_0 w_1 w_2')\n",
    "w = Matrix(3, 1, w)\n",
    "R = MatrixSymbol('R', 3, 3)\n",
    "w_skew = Matrix(3, 3, [0, -w[2], w[1], w[2], 0, -w[0], -w[1], w[0], 0])\n",
    "w_skew"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{1}{\\sqrt{\\left|{w_{0}}\\right|^{2} + \\left|{w_{1}}\\right|^{2} + \\left|{w_{2}}\\right|^{2}}} \\left[\\begin{matrix}0 & - w_{0} w_{2} + w_{0} R_{1, 0} + w_{1} \\left(1 - R_{0, 0}\\right) & w_{0} w_{1} + w_{0} R_{2, 0} + w_{2} \\left(1 - R_{0, 0}\\right)\\\\w_{0} w_{2} - w_{0} R_{1, 0} - w_{1} \\left(1 - R_{0, 0}\\right) & 0 & - w_{0}^{2} + w_{1} R_{2, 0} - w_{2} R_{1, 0}\\\\- w_{0} w_{1} - w_{0} R_{2, 0} - w_{2} \\left(1 - R_{0, 0}\\right) & w_{0}^{2} - w_{1} R_{2, 0} + w_{2} R_{1, 0} & 0\\end{matrix}\\right] R$"
      ],
      "text/plain": [
       "                           ⎡              0                 -w₀⋅w₂ + w₀⋅R₁₀ + \n",
       "                           ⎢                                                  \n",
       "            1              ⎢                                                  \n",
       "──────────────────────────⋅⎢w₀⋅w₂ - w₀⋅R₁₀ - w₁⋅(1 - R₀₀)                 0   \n",
       "   _______________________ ⎢                                                  \n",
       "  ╱     2       2       2  ⎢                                      2           \n",
       "╲╱  │w₀│  + │w₁│  + │w₂│   ⎣-w₀⋅w₁ - w₀⋅R₂₀ - w₂⋅(1 - R₀₀)      w₀  - w₁⋅R₂₀ +\n",
       "\n",
       "w₁⋅(1 - R₀₀)  w₀⋅w₁ + w₀⋅R₂₀ + w₂⋅(1 - R₀₀)⎤  \n",
       "                                           ⎥  \n",
       "                     2                     ⎥  \n",
       "                 - w₀  + w₁⋅R₂₀ - w₂⋅R₁₀   ⎥⋅R\n",
       "                                           ⎥  \n",
       "                                           ⎥  \n",
       " w₂⋅R₁₀                     0              ⎦  "
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "v = (w_skew * (eye(3) - R)*eye(3)[:, 0])\n",
    "v = Matrix(3, 1, Matrix(v))\n",
    "v_skew = Matrix(3, 3, [0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0])\n",
    "dRdv0 = ((w[0] * w_skew + v_skew) * R) / w.norm(2)\n",
    "dRdv0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{1}{\\sqrt{\\left|{w_{0}}\\right|^{2} + \\left|{w_{1}}\\right|^{2} + \\left|{w_{2}}\\right|^{2}}} \\left[\\begin{matrix}0 & - w_{0} \\left(1 - R_{1, 1}\\right) - w_{1} w_{2} - w_{1} R_{0, 1} & w_{0} R_{2, 1} + w_{1}^{2} - w_{2} R_{0, 1}\\\\w_{0} \\left(1 - R_{1, 1}\\right) + w_{1} w_{2} + w_{1} R_{0, 1} & 0 & - w_{0} w_{1} + w_{1} R_{2, 1} + w_{2} \\left(1 - R_{1, 1}\\right)\\\\- w_{0} R_{2, 1} - w_{1}^{2} + w_{2} R_{0, 1} & w_{0} w_{1} - w_{1} R_{2, 1} - w_{2} \\left(1 - R_{1, 1}\\right) & 0\\end{matrix}\\right] R$"
      ],
      "text/plain": [
       "                           ⎡                                                  \n",
       "                           ⎢              0                -w₀⋅(1 - R₁₁) - w₁⋅\n",
       "            1              ⎢                                                  \n",
       "──────────────────────────⋅⎢w₀⋅(1 - R₁₁) + w₁⋅w₂ + w₁⋅R₀₁                0    \n",
       "   _______________________ ⎢                                                  \n",
       "  ╱     2       2       2  ⎢               2                                  \n",
       "╲╱  │w₀│  + │w₁│  + │w₂│   ⎣   -w₀⋅R₂₁ - w₁  + w₂⋅R₀₁      w₀⋅w₁ - w₁⋅R₂₁ - w₂\n",
       "\n",
       "                            2              ⎤  \n",
       "w₂ - w₁⋅R₀₁      w₀⋅R₂₁ + w₁  - w₂⋅R₀₁     ⎥  \n",
       "                                           ⎥  \n",
       "             -w₀⋅w₁ + w₁⋅R₂₁ + w₂⋅(1 - R₁₁)⎥⋅R\n",
       "                                           ⎥  \n",
       "                                           ⎥  \n",
       "⋅(1 - R₁₁)                 0               ⎦  "
      ]
     },
     "execution_count": 37,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "v = (w_skew * (eye(3) - R)*eye(3)[:, 1])\n",
    "v = Matrix(3, 1, Matrix(v))\n",
    "v_skew = Matrix(3, 3, [0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0])\n",
    "dRdv1 = ((w[1] * w_skew + v_skew) * R) / w.norm(2)\n",
    "dRdv1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{1}{\\sqrt{\\left|{w_{0}}\\right|^{2} + \\left|{w_{1}}\\right|^{2} + \\left|{w_{2}}\\right|^{2}}} \\left[\\begin{matrix}0 & w_{0} R_{1, 2} - w_{1} R_{0, 2} - w_{2}^{2} & - w_{0} \\left(1 - R_{2, 2}\\right) + w_{1} w_{2} - w_{2} R_{0, 2}\\\\- w_{0} R_{1, 2} + w_{1} R_{0, 2} + w_{2}^{2} & 0 & - w_{0} w_{2} - w_{1} \\left(1 - R_{2, 2}\\right) - w_{2} R_{1, 2}\\\\w_{0} \\left(1 - R_{2, 2}\\right) - w_{1} w_{2} + w_{2} R_{0, 2} & w_{0} w_{2} + w_{1} \\left(1 - R_{2, 2}\\right) + w_{2} R_{1, 2} & 0\\end{matrix}\\right] R$"
      ],
      "text/plain": [
       "                           ⎡                                                  \n",
       "                           ⎢              0                    w₀⋅R₁₂ - w₁⋅R₀₂\n",
       "            1              ⎢                                                  \n",
       "──────────────────────────⋅⎢                        2                         \n",
       "   _______________________ ⎢   -w₀⋅R₁₂ + w₁⋅R₀₂ + w₂                     0    \n",
       "  ╱     2       2       2  ⎢                                                  \n",
       "╲╱  │w₀│  + │w₁│  + │w₂│   ⎣w₀⋅(1 - R₂₂) - w₁⋅w₂ + w₂⋅R₀₂  w₀⋅w₂ + w₁⋅(1 - R₂₂\n",
       "\n",
       "     2                                    ⎤  \n",
       " - w₂       -w₀⋅(1 - R₂₂) + w₁⋅w₂ - w₂⋅R₀₂⎥  \n",
       "                                          ⎥  \n",
       "                                          ⎥⋅R\n",
       "            -w₀⋅w₂ - w₁⋅(1 - R₂₂) - w₂⋅R₁₂⎥  \n",
       "                                          ⎥  \n",
       ") + w₂⋅R₁₂                0               ⎦  "
      ]
     },
     "execution_count": 40,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "v = (w_skew * (eye(3) - R)*eye(3)[:, 2])\n",
    "v = Matrix(3, 1, Matrix(v))\n",
    "v_skew = Matrix(3, 3, [0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0])\n",
    "dRdv2 = ((w[2] * w_skew + v_skew) * R) / w.norm(2)\n",
    "dRdv2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{f_{x}}{x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}} & 0 & - \\frac{f_{x} \\left(x_{w} R_{0, 0} + y_{w} R_{0, 1} + z_{w} R_{0, 2} + t_{0, 0}\\right)}{\\left(x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}\\right)^{2}}\\\\0 & \\frac{f_{y}}{x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}} & - \\frac{f_{y} \\left(x_{w} R_{1, 0} + y_{w} R_{1, 1} + z_{w} R_{1, 2} + t_{1, 0}\\right)}{\\left(x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}\\right)^{2}}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[f_x/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0]),                                                       0, -f_x*(x_w*R[0, 0] + y_w*R[0, 1] + z_w*R[0, 2] + t[0, 0])/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0])**2],\n",
       "[                                                      0, f_y/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0]), -f_y*(x_w*R[1, 0] + y_w*R[1, 1] + z_w*R[1, 2] + t[1, 0])/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0])**2]])"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "f_x, f_y, p_x, p_y = symbols('f_x f_y p_x p_y')\n",
    "x_w, y_w, z_w = symbols('x_w y_w z_w')\n",
    "\n",
    "X_w = Matrix(3, 1, [x_w, y_w, z_w])\n",
    "K = Matrix(3, 3, [f_x, 0, p_x, 0, f_y, p_y, 0, 0, 1])\n",
    "Rv = Matrix(9, 1, Matrix(R))\n",
    "t = MatrixSymbol('t', 3, 1)\n",
    "X_i_homo = Matrix(K * (R * X_w + t))\n",
    "X_i_homo\n",
    "X_i = Matrix(X_i_homo[0:2, 0] / X_i_homo[2, 0])\n",
    "\n",
    "dxdt = X_i.jacobian(t).applyfunc(simplify)\n",
    "dxdt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{f_{x} x_{w}}{x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}} & \\frac{f_{x} y_{w}}{x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}} & \\frac{f_{x} z_{w}}{x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}} & 0 & 0 & 0 & - \\frac{f_{x} x_{w} \\left(x_{w} R_{0, 0} + y_{w} R_{0, 1} + z_{w} R_{0, 2} + t_{0, 0}\\right)}{\\left(x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}\\right)^{2}} & - \\frac{f_{x} y_{w} \\left(x_{w} R_{0, 0} + y_{w} R_{0, 1} + z_{w} R_{0, 2} + t_{0, 0}\\right)}{\\left(x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}\\right)^{2}} & - \\frac{f_{x} z_{w} \\left(x_{w} R_{0, 0} + y_{w} R_{0, 1} + z_{w} R_{0, 2} + t_{0, 0}\\right)}{\\left(x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}\\right)^{2}}\\\\0 & 0 & 0 & \\frac{f_{y} x_{w}}{x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}} & \\frac{f_{y} y_{w}}{x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}} & \\frac{f_{y} z_{w}}{x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}} & - \\frac{f_{y} x_{w} \\left(x_{w} R_{1, 0} + y_{w} R_{1, 1} + z_{w} R_{1, 2} + t_{1, 0}\\right)}{\\left(x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}\\right)^{2}} & - \\frac{f_{y} y_{w} \\left(x_{w} R_{1, 0} + y_{w} R_{1, 1} + z_{w} R_{1, 2} + t_{1, 0}\\right)}{\\left(x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}\\right)^{2}} & - \\frac{f_{y} z_{w} \\left(x_{w} R_{1, 0} + y_{w} R_{1, 1} + z_{w} R_{1, 2} + t_{1, 0}\\right)}{\\left(x_{w} R_{2, 0} + y_{w} R_{2, 1} + z_{w} R_{2, 2} + t_{2, 0}\\right)^{2}}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[f_x*x_w/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0]), f_x*y_w/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0]), f_x*z_w/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0]),                                                           0,                                                           0,                                                           0, -f_x*x_w*(x_w*R[0, 0] + y_w*R[0, 1] + z_w*R[0, 2] + t[0, 0])/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0])**2, -f_x*y_w*(x_w*R[0, 0] + y_w*R[0, 1] + z_w*R[0, 2] + t[0, 0])/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0])**2, -f_x*z_w*(x_w*R[0, 0] + y_w*R[0, 1] + z_w*R[0, 2] + t[0, 0])/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0])**2],\n",
       "[                                                          0,                                                           0,                                                           0, f_y*x_w/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0]), f_y*y_w/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0]), f_y*z_w/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0]), -f_y*x_w*(x_w*R[1, 0] + y_w*R[1, 1] + z_w*R[1, 2] + t[1, 0])/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0])**2, -f_y*y_w*(x_w*R[1, 0] + y_w*R[1, 1] + z_w*R[1, 2] + t[1, 0])/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0])**2, -f_y*z_w*(x_w*R[1, 0] + y_w*R[1, 1] + z_w*R[1, 2] + t[1, 0])/(x_w*R[2, 0] + y_w*R[2, 1] + z_w*R[2, 2] + t[2, 0])**2]])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dxdRv = X_i.jacobian(Rv).applyfunc(simplify)\n",
    "dxdRv"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.4"
  },
  "latex_envs": {
   "LaTeX_envs_menu_present": true,
   "autoclose": false,
   "autocomplete": true,
   "bibliofile": "biblio.bib",
   "cite_by": "apalike",
   "current_citInitial": 1,
   "eqLabelWithNumbers": true,
   "eqNumInitial": 1,
   "hotkeys": {
    "equation": "Ctrl-E",
    "itemize": "Ctrl-I"
   },
   "labels_anchors": false,
   "latex_user_defs": false,
   "report_style_numbering": false,
   "user_envs_cfg": false
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
